%**************************************************************************
%BatBot: Biological inspired Bat roBot.

%Copyright RObotics and Cybernetics Group
%Julian Colorado

% Matlab simulator of bat flight behavior. 
%**************************************************************************


clear all 
clc 
clf 
%a0 = 1.5; % Shoulder joint radius, [mm]
a0 = 20;

%a1 = 21.5;%29; % Humerus length, [mm] 
a1 = 55;

%a2 = 40;% radius length, [mm]
a2 = 70;

%Ld = 55;% Length of the 3rd digit [mm] 
Ld = 110;

xtip3 = Ld*cos(31.68*pi/180); % x coord of the wingtip relative to x3y3z3 frame, [mm] 
ztip3 = -Ld*sin(31.68*pi/180); % z coord of the wingtip relative to x3y3z3 frame, [mm] 
gm = 58*pi/180; % the inclination angle of the stroke plane 
figure(1) 
fig = figure(1); 
set(fig,'DoubleBuffer','on'); 
set(gca,'xlim',[0 1],'ylim',[60 160],'nextplot','replace','Visible','on'); 
hold on 
fps = 4; 
aviobj = avifile('Wing_tip_ANIMATION.avi','fps',fps,'quality',100);
for i = 1:84 
    t = 0.001*(i);
    th1 = (5+35*cos(2*pi/0.084*t-0*pi/180))*pi/180; th2 = (55+30*cos(2*pi/0.084*t+65*pi/180))*pi/180; th3 = (-10+40*cos(2*pi/0.084*t-80*pi/180))*pi/180;
    theta1(i) = th1;
    xtip = ((-cos(gm)*sin(th1)*cos(th2)+sin(gm)*sin(th2))*cos(th3)- cos(gm)...
    *cos(th1)*sin(th3))*xtip3+(cos(gm)*sin(th1)*sin(th2)+sin(gm)*... 
    cos(th2))*ztip3+(-cos(gm)*sin(th1)*cos(th2)+sin(gm)*sin(th2))*... 
    a2-cos(gm)*sin(th1)*a1;

    ytip = (cos(th1)*cos(th2)*cos(th3)-sin(th1)*sin(th3))*xtip3- cos(th1)*...
            sin(th2)*ztip3+cos(th1)*cos(th2)*a2+cos(th1)*a1+a0; 
   
    ztip = ((sin(gm)*sin(th1)*cos(th2)+cos(gm)*sin(th2))*cos(th3)+sin(gm)*... 
           cos(th1)*sin(th3))*xtip3+(-sin(gm)*sin(th1)*sin(th2)+cos(gm)*...
           cos(th2))*ztip3+(sin(gm)*sin(th1)*cos(th2)+cos(gm)*sin(th2))*a2+... 
           sin(gm)*sin(th1)*a1;

       plot3(xtip,ytip,ztip,'.r','markersize',20) 
       hold on 
       plot3(-40,ytip,ztip,'ob','markerfacecolor','b','markersize',2) 
       hold on
       plot3(xtip,60,ztip,'*m','markersize',5) 
       hold on
       plot3(xtip,ytip,-40,'dk','markerfacecolor','k','markersize',5) 
       set(gca,'DrawMode','fast')
       frame = getframe(fig);
       title('\bf{Wingtip trajectory - model}','FontSize',18) 
       xlabel('\bf{x_b, [mm]}','FontSize',18) 
       ylabel('\bf{y_b, [mm]}','FontSize',18) 
       zlabel('\bf{z_b, [mm]}','FontSize',18) 
       set(gca,'LineWidth',2,'FontSize',18)
       axis([-40 80 60 140 -40 100]); 
       set(gca,'ZTick',[-40;-20;0;20;40;60;80;100]) 
       grid on 
       view([138 12]) 
       aviobj = addframe(aviobj,frame);
end
th1
aviobj = close(aviobj); 
disp('Movie Finished');
